#include "Primitive.h"

double Primitive::myMax(double a, double b, double c){
	if(a>=b && a>=c) return a;
	else if(b>=a && b>=c) return b;
	else if(c>=a && c>=b) return c;
	return -1;
}

void Primitive::init(Vec3d _mOffset, Vec3d _mDim, double Mass){
	mDim = _mDim;
	mOffset = _mOffset;
	mCOM = mOffset;
	mID = primCount++;
	mMass=Mass;
	mMassInv=1.0/Mass;
	mMassTensor=NULL;
	mInertia=NULL;
	mInertiaInv=NULL;
	mBody=NULL;
}

Primitive::Primitive(){
	init(vl_0, vl_0, 0);
}

Primitive::Primitive( double Mass ){
	init(vl_0, vl_0, Mass);
}

Primitive::Primitive(Vec3d _mOffset, Vec3d _mDim, double Mass){
	init(_mOffset, _mDim, Mass);
}

void Primitive::setMassTensorFromInertia(double mat[3][3]){
	if(mMassTensor!=NULL) delete mMassTensor;
	mMassTensor=NULL;
	if(mat==NULL){
		mMassTensor=getMassTensor();
	}
	else {
		mMassTensor = new Mat4d(vl_0);
		// compute the half trace of mat = row*integral((x*x+y*y+z*z)dxdydz) = sum of moment of inertia along all 3 axes
		double halftrace = 0.5*(mat[0][0]+mat[1][1]+mat[2][2]);
		for(int i=0; i<3; i++)
			for(int j=0; j<3; j++)
                (*mMassTensor)[i][j] = -mat[i][j];
		(*mMassTensor)[0][0]+=halftrace;
		(*mMassTensor)[1][1]+=halftrace;
		(*mMassTensor)[2][2]+=halftrace;
		(*mMassTensor)[3][3]=mMass;
	}
}


// get the inertia matrix from mass tensor already computed
Mat3d *Primitive::getInertia(){
	if(mInertia==NULL){
		mInertia= new Mat3d(vl_0);
		Mat4d *mat = getMassTensor();
		double mTrace = ((*mat)[0][0]+ (*mat)[1][1]+ (*mat)[2][2]);
		for(int i=0; i<3; i++)
			for(int j=0; j<3; j++)
                (*mInertia)[i][j] = -(*mat)[i][j];
		(*mInertia)[0][0]+=mTrace;
		(*mInertia)[1][1]+=mTrace;
		(*mInertia)[2][2]+=mTrace;
	}
	return mInertia;
}

void Primitive::setMass(double _m){
	// change the inertia matrices
	if(mMassTensor) (*mMassTensor)*=_m/mMass;
	if(mInertia) (*mInertia)*=_m/mMass;
	if(mInertiaInv) (*mInertiaInv)*=mMass/_m;

	// store mass/invmass
	mMassInv = 1/_m;
	mMass=_m;
}

void Primitive::setInertia(double inertia[3][3])
{
	if (mInertia==NULL)
		mInertia= new Mat3d(vl_0);
	for (int i = 0; i < 3; ++i)
		for (int j = 0; j < 3; ++j)
			(*mInertia)[i][j] = inertia[i][j];
	if(mInertiaInv==NULL)
		mInertiaInv = new Mat3d(vl_0);
		*mInertiaInv = inv(*mInertia);
	setMassTensorFromInertia(inertia);
}
